#include "moco_protocal.h"

#include <Arduino.h>
#include <SoftwareSerial.h>
#include <MsTimer2.h>

// #define DEBUG_PRINT_TX_DATA
// #define DEBUG_PRINT_RX_DATA
// TODO 设定一个最高限幅器
#define LIMIT(v, min, max) ((v) > (max) ? (max) : ((v) < (min) ? (min) : (v)))

SoftwareSerial *debugSerial;

moco_rx_protocal_t moco_rx_data;
uint8_t rx_len = 0;
uint8_t moco_rx_data_state = STA_NOT_READY;

moco_tx_protocal_t moco_tx_data;

uint8_t moco_decode_protocal_data(uint8_t byte_data);
void moco_send_data(moco_tx_protocal_t *tx_data);
void moco_period_send(void);
void wait_for_moco_stable(void);

void moco_init()
{
    Serial.begin(115200);
    // debugSerial = (SoftwareSerial *)serial;

    MsTimer2::set(MOCO_PROTOCAL_PRIOD_SEND_INTERVAL, moco_period_send);
    MsTimer2::start();

    wait_for_moco_stable();
}

void wait_for_moco_stable(void)
{
    while (1)
    {
        moco_recive_data();
        delay(100);
        if (moco_rx_data.bat_percent != 0)
        {
            break;
        }
    }
}

// send data periodically to Moco, moco request data at last every 500ms otherwise
// it will enter idle mode.
void moco_period_send(void)
{
    moco_send_data(&moco_tx_data);
}

void moco_recive_data()
{
    uint16_t timeout = 0;

    // clear buffer first
    while (Serial.available())
    {
        Serial.read();
    }
    moco_rx_data_state = STA_NOT_READY;

    // wait for head data
    while (1)
    {
        if (Serial.peek() == MOCO_RX_PROTOCAL_START0)
        {
            break;
        }
        else if (Serial.available())
        {
            Serial.read();
        }
    }

    // wait until data is ready or timeout
    while (moco_rx_data_state == STA_NOT_READY || moco_rx_data_state == STA_ERROR)
    {
        delay(1);
        // read data
        while (Serial.available())
        {
            moco_rx_data_state = moco_decode_protocal_data(Serial.read());
            if (moco_rx_data_state == STA_OK)
            {
                break;
            }
        }
        timeout++;
        if (timeout > 100)
        {
            moco_rx_data_state = STA_ERROR;
            break;
        }
    }
}

void moco_rx_buffer_push(uint8_t data)
{
    *((uint8_t *)&moco_rx_data + rx_len) = data;
    rx_len++;
}

uint8_t moco_decode_protocal_data(uint8_t byte_data)
{
    // get MOCO_PROTOCAL_START0
    if (byte_data == MOCO_RX_PROTOCAL_START0 && rx_len == 0)
    {
        moco_rx_buffer_push(byte_data);
    }
    // get MOCO_PROTOCAL_START1
    else if (byte_data == MOCO_RX_PROTOCAL_START1 && rx_len == 1)
    {
        moco_rx_buffer_push(byte_data);
    }
    // get MOCO_PROTOCAL_START2
    else if (byte_data == MOCO_RX_PROTOCAL_ID && rx_len == 2)
    {
        moco_rx_buffer_push(byte_data);
    }
    // get data length
    else if (rx_len == 3)
    {
        moco_rx_buffer_push(byte_data);
    }
    // get data and checksum
    else if (rx_len > 3 && (rx_len < (moco_rx_data.data_len + 5)))
    {
        moco_rx_buffer_push(byte_data);
    }
    else
    {
        rx_len = 0;
        return STA_ERROR;
    }

    // Data recive finish
    if (rx_len == (moco_rx_data.data_len + 5))
    {
#ifdef DEBUG_PRINT_RX_DATA
        // For debug: print recive data in hex format
        debugSerial->print("Data recived: ");
        for (uint8_t i = 0; i < rx_len; i++)
        {
            char hex[1];
            sprintf(hex, "%02X", *((uint8_t *)&moco_rx_data + i));
            debugSerial->print(hex);
            debugSerial->print(" ");
        }
        debugSerial->println();
#endif
        // Check checksum
        uint8_t checksum = 0;
        for (uint8_t i = 0; i < (rx_len - 1); i++)
        {
            checksum += *((uint8_t *)&moco_rx_data + i);
        }
        if (checksum == moco_rx_data.checksum)
        {
            rx_len = 0;
            return STA_OK;
        }
        else
        {
            rx_len = 0;
            return STA_ERROR;
        }
    }
    else
    {
        return STA_NOT_READY;
    }
}

void moco_send_data(moco_tx_protocal_t *tx_data)
{
    uint8_t tx_len = sizeof(moco_tx_protocal_t);

    tx_data->start_byte[0] = MOCO_TX_PROTOCAL_START0;
    tx_data->start_byte[1] = MOCO_TX_PROTOCAL_START1;

    tx_data->data_len = tx_len - 5;

    uint8_t checksum = 0;
    for (uint8_t i = 0; i < (tx_len - 1); i++)
    {
        checksum += *((uint8_t *)tx_data + i);
    }
    tx_data->checksum = checksum;
    Serial.write((uint8_t *)tx_data, tx_len);

#ifdef DEBUG_PRINT_TX_DATA
    debugSerial->print("Data sended: ");
    for (uint8_t j = 0; j < tx_len; j++)
    {
        char hex[1];
        sprintf(hex, "%02X", *((uint8_t *)tx_data + j));
        debugSerial->print(hex);
        debugSerial->print(" ");
    }
    debugSerial->println();
#endif
}

void moco_set_gail(Gait_Mode mode)
{
    // if current gait is trot or walk, and setting to MOCO_TX_STAND_RC we should send MOCO_TX_STAND_IMU instead
    if(moco_tx_data.gait == MOCO_TX_TROT || moco_tx_data.gait == MOCO_TX_F_TROT || moco_tx_data.gait == MOCO_TX_WALK)
    {
        if(mode == MOCO_TX_STAND_RC)
        mode = MOCO_TX_STAND_IMU;
    }

    moco_tx_data.id = MOCO_TX_PROTOCAL_SITE_MODE_ID;
    moco_tx_data.gait = mode;

    // if altitude lower than 0.7 set to 0.7, so that the trot mode can be set
    if (mode == MOCO_TX_TROT)
    {
        moco_tx_data.altitude = LIMIT(moco_tx_data.altitude, 0.7001, 1.0);
    }
    moco_send_data(&moco_tx_data);
    delay(MOCO_GAIT_INTERVAL);
}

void moco_set_altitude(float altitude)
{
    altitude = LIMIT(altitude, 0.0, 1.0);

    // prevent altitude lower than 0.7 in trot mode
    if (moco_tx_data.gait == MOCO_TX_TROT)
    {
        altitude = LIMIT(altitude, 0.7001, 1.0);
    }

    moco_tx_data.id = MOCO_TX_PROTOCAL_SITE_MODE_ID;
    moco_tx_data.altitude = altitude;
    moco_send_data(&moco_tx_data);
}

void moco_set_x_speed(float speed)
{
    speed = LIMIT(speed, MOCO_X_SPEED_MIN, MOCO_X_SPEED_MAX);

    moco_tx_data.id = MOCO_TX_PROTOCAL_SITE_MODE_ID;
    moco_tx_data.x_speed = speed;
    moco_send_data(&moco_tx_data);
}

void moco_set_y_speed(float speed)
{
    speed = LIMIT(speed, MOCO_Y_SPEED_MIN, MOCO_Y_SPEED_MAX);

    moco_tx_data.id = MOCO_TX_PROTOCAL_SITE_MODE_ID;
    moco_tx_data.y_speed = speed;
    moco_send_data(&moco_tx_data);
}

void moco_set_turn_speed(float speed)
{
    speed = LIMIT(speed, MOCO_T_SPEED_MIN, MOCO_T_SPEED_MAX);

    moco_tx_data.id = MOCO_TX_PROTOCAL_SITE_MODE_ID;
    moco_tx_data.turn_speed = speed;
    moco_send_data(&moco_tx_data);
}

void moco_set_speed(float x_speed, float y_speed, float turn_speed)
{
    moco_set_x_speed(x_speed);
    moco_set_y_speed(y_speed);
    moco_set_turn_speed(turn_speed);
}

void moco_set_angle(float pitch, float roll, float yaw)
{
    pitch = LIMIT(pitch, MOCO_PITCH_ANGLE_MIN, MOCO_PITCH_ANGLE_MAX);
    roll = LIMIT(roll, MOCO_ROLL_ANGLE_MIN, MOCO_ROLL_ANGLE_MAX);
    yaw = LIMIT(yaw, MOCO_YAW_ANGLE_MIN, MOCO_YAW_ANGLE_MAX);

    moco_tx_data.id = MOCO_TX_PROTOCAL_SITE_MODE_ID;
    moco_tx_data.pitch_angle = pitch;
    moco_tx_data.roll_angle = roll;
    moco_tx_data.yawn_angle = yaw;
    moco_send_data(&moco_tx_data);
}

void moco_run_motion_group(uint8_t group_id)
{
    moco_tx_data.id = MOCO_TX_PROTOCAL_SITE_MODE_ID;
    moco_tx_data.motion_group_enable = 1;
    moco_tx_data.motion_group_number = group_id;
    moco_send_data(&moco_tx_data);
}

void moco_disable_motion_group()
{
    moco_tx_data.id = MOCO_TX_PROTOCAL_SITE_MODE_ID;
    moco_tx_data.motion_group_enable = 0;
    moco_send_data(&moco_tx_data);
}

void moco_run_and_wait_for_motion_group_finish(uint8_t group_id)
{
    moco_run_motion_group(group_id);

    while (1)
    {
        moco_recive_data();
        if (moco_rx_data_state == STA_OK)
        {
            if(moco_rx_data.motion_group == MOCO_MOTION_GROUP_FINISH){
                moco_disable_motion_group();
                break;
            }
        }
        delay(1);
    }
}